/*
**函数信息      KalmanFilter
**功能描述      卡尔曼一阶滤波
**输入参数      Input       输入数组
                Len         数组长度
                Output      输出缓冲
                Q           卡尔曼参数Q      Q越小越信赖模型
                R           卡尔曼参数R
**输出参数      none
*/
void KalmanFilter(volatile INT16U *Input, INT16U Len, INT16U *Output, FP32 Q, FP32 R)
{
    FP32 x_last = 0;
	FP32 p_last = 0.8;
	FP32 kg;
	FP32 x_mid;
	FP32 x_now;
	FP32 p_mid;
	FP32 p_now;
    INT16U ii;

    x_last = (FP32)Input[0];

    for(ii = 0 ; ii < Len ; ii++)
    {
        x_mid = x_last;
        p_mid = p_last + Q;
        kg = p_mid / (p_mid + R);
        x_now = x_mid + kg*((FP32)Input[ii] - x_mid);//估计出的最有值
        p_now = (1 - kg)*p_mid;//最优值对应的协方差

        p_last = p_now;
        x_last = x_now;

        Output[ii] = (INT16U)x_now;
    }
}

